#include "stdafx.h"
#include "../include/GcSurfaceCalculator.h"
#include "../include/GcConstVariable.h"
#include "math.h"

using namespace GeoCalculator;

GcSurfaceCalculator::GcSurfaceCalculator(bool rad)
	: GcGeoCalculator(rad)
{
}

GcSurfaceCalculator::GcSurfaceCalculator(double srcLon, double srcLat, double dstLon, double dstLat, bool rad)
	: GcGeoCalculator(srcLon, srcLat, GcConstVariable::radius, dstLon, dstLat, GcConstVariable::radius,rad)
{
}


GcSurfaceCalculator::~GcSurfaceCalculator()
{
}

void GcSurfaceCalculator::setSrcVertex(double lon, double lat, bool rad)
{
	if (_rad != rad)
	{
		if (_rad)
		{
			_srcVertex.setX(GcConstVariable::deg2rad(lon));
			_srcVertex.setY(GcConstVariable::deg2rad(lat));
		}
		else
		{
			_srcVertex.setX(GcConstVariable::rad2deg(lon));
			_srcVertex.setY(GcConstVariable::rad2deg(lat));
		}
		
		_srcVertex.setZ(GcConstVariable::radius);
	}
	else _srcVertex.setXYZ(lon, lat, GcConstVariable::radius);

	update();
}

void GcSurfaceCalculator::setDstVertex(double lon, double lat, bool rad)
{
	if (_rad != rad)
	{
		if (_rad)
		{
			_dstVertex.setX(GcConstVariable::deg2rad(lon));
			_dstVertex.setY(GcConstVariable::deg2rad(lat));
		}
		else
		{
			_dstVertex.setX(GcConstVariable::rad2deg(lon));
			_dstVertex.setY(GcConstVariable::rad2deg(lat));
		}

		_dstVertex.setZ(GcConstVariable::radius);
	}
	else _dstVertex.setXYZ(lon, lat, GcConstVariable::radius);

	update();
}

double GcSurfaceCalculator::distance()
{
	double a_rad, b_rad, x_rad, y_rad;
	if (!_rad)
	{
		a_rad = GcConstVariable::deg2rad(_srcVertex.x());
		b_rad = GcConstVariable::deg2rad(_srcVertex.y());
		x_rad = GcConstVariable::deg2rad(_dstVertex.x());
		y_rad = GcConstVariable::deg2rad(_dstVertex.y());
	}
	
	double lon = a_rad - x_rad;
	double lat = b_rad - y_rad;

	double s = 2 * asin(sqrt(pow(sin(lat / 2), 2) + cos(b_rad)* cos(y_rad)* pow(sin(lon / 2), 2)));
	s = s * GcConstVariable::radius;
	return s;
}

double GcSurfaceCalculator::distance(double lon, double lat, bool rad)
{
	double la, lb, lc;
	la = distance();
	
	GcSurfaceCalculator surfaceCalculator(false);
	surfaceCalculator.setSrcVertex(_srcVertex.x(), _srcVertex.y(), _rad);
	surfaceCalculator.setDstVertex(lon, lat, rad);
	lb = surfaceCalculator.distance();
	surfaceCalculator.setSrcVertex(_dstVertex.x(), _dstVertex.y(), _rad);
	lc = surfaceCalculator.distance();

	if (lb*lb >= lc*lc + la*la)
		return lc;
	if (lc*lc >= lb*lb + la*la)
		return lb;
	double l = (la + lb + lc) / 2;  
	double s = sqrt(l*(l - la)*(l - lb)*(l - lc));
	return 2 * s / la;
}

void GcSurfaceCalculator::position(double angle, double distance, double &lon, double &lat, bool rad)
{
	double angle_rad, b_rad, c_rad;
	if (!rad)
		angle_rad = GcConstVariable::deg2rad(angle);
	else angle_rad = angle;

	if (!_rad)
	{
		b_rad = GcConstVariable::deg2rad(_srcVertex.x());
		c_rad = GcConstVariable::deg2rad(_srcVertex.y());
	}
	else
	{
		b_rad = _srcVertex.x();
		c_rad = _srcVertex.y();
	}

	double sinAlpha1 = sin(angle_rad);
	double cosAlpha1 = cos(angle_rad);

	double tanU1 = (1 - GcConstVariable::f) * tan(b_rad);
	double cosU1 = 1 / sqrt((1 + tanU1 * tanU1));
	double sinU1 = tanU1 * cosU1;
	double sigma1 = atan2(tanU1, cosAlpha1);
	double sinAlpha = cosU1 * sinAlpha1;
	double cosSqAlpha = 1 - sinAlpha * sinAlpha;
	double uSq = cosSqAlpha * (GcConstVariable::a * GcConstVariable::a - GcConstVariable::b * GcConstVariable::b) / (GcConstVariable::b * GcConstVariable::b);
	double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
	double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));

	double cos2SigmaM = 0;
	double sinSigma = 0;
	double cosSigma = 0;
	double sigma = distance / (GcConstVariable::b * A), sigmaP = 2 * GcConstVariable::pi;
	while (fabs(sigma - sigmaP) > 1e-12) {
		cos2SigmaM = cos(2 * sigma1 + sigma);
		sinSigma = sin(sigma);
		cosSigma = cos(sigma);
		double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)
			- B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
		sigmaP = sigma;
		sigma = distance / (GcConstVariable::b * A) + deltaSigma;
	}

	double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
	double lat2 = atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
		(1 - GcConstVariable::f) * sqrt(sinAlpha * sinAlpha + tmp * tmp));
	double lambda = atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1);
	double C = GcConstVariable::f / 16 * cosSqAlpha * (4 + GcConstVariable::f * (4 - 3 * cosSqAlpha));
	double L = lambda - (1 - C) * GcConstVariable::f * sinAlpha
		* (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));

	lon = c_rad + L;
	if(!rad)
		lon = GcConstVariable::rad2deg(lon);
	lat = lat2;
	if (!rad)
		lat = GcConstVariable::rad2deg(lat);
}
